#ifndef BTL_CAMERA_CAMERAMODELPLUMBBOB
#define BTL_CAMERA_CAMERAMODELPLUMBBOB

#include <btl/Camera/CameraModel.hpp>

namespace btl {
namespace camera {

class CameraModelPlumbBob : public CameraModelPinhole, public CameraModelDistorted
{

   public:

      CameraModelPlumbBob(
         int imageResolutionX, int imageResolutionY,
         double focalLength,
         double principlePointX, double principlePointY,
         double k1, double k2, double k3, double k4, double k5);

      CameraModelPlumbBob(
         const Eigen::Vector2i& imageResolution,
         double focalLength,
         const Eigen::Vector2d& principlePoint,
         double k1, double k2, double k3, double k4, double k5);

      CameraModelPlumbBob(
         int imageResolutionX, int imageResolutionY,
         double focalLengthX, double focalLengthY,
         double principlePointX, double principlePointY,
         double k1, double k2, double k3, double k4, double k5);

      CameraModelPlumbBob(
         const Eigen::Vector2i& imageResolution,
         const Eigen::Vector2d& focalLength,
         const Eigen::Vector2d& principlePoint,
         double k1, double k2, double k3, double k4, double k5);

      ~CameraModelPlumbBob();

      Eigen::Vector2d distortPoint(const Eigen::Vector2d& undistorted);

   protected:

      double _k1, _k2, _k3, _k4, _k5;

};

} //namespace camera;
} //namespace btl

namespace btl {

   using camera::CameraModelPlumbBob;

} //namespace btl

namespace btl {
namespace camera {

inline CameraModelPlumbBob::CameraModelPlumbBob(
   int imageResolutionX, int imageResolutionY,
   double focalLength,
   double principlePointX, double principlePointY,
   double k1, double k2, double k3, double k4, double k5
) :
   CameraModelPinhole(imageResolutionX, imageResolutionY, focalLength, principlePointX, principlePointY),
   _k1(k1), _k2(k2), _k3(k3), _k4(k4), _k5(k5)
{}

inline CameraModelPlumbBob::CameraModelPlumbBob(
   const Eigen::Vector2i& imageResolution,
   double focalLength,
   const Eigen::Vector2d& principlePoint,
   double k1, double k2, double k3, double k4, double k5
) :
   CameraModelPinhole(imageResolution, focalLength, principlePoint),
   _k1(k1), _k2(k2), _k3(k3), _k4(k4), _k5(k5)
{}

inline CameraModelPlumbBob::CameraModelPlumbBob(
   int imageResolutionX, int imageResolutionY,
   double focalLengthX, double focalLengthY,
   double principlePointX, double principlePointY,
   double k1, double k2, double k3, double k4, double k5
) :
   CameraModelPinhole(imageResolutionX, imageResolutionY, focalLengthX, focalLengthY, principlePointX, principlePointY),
   _k1(k1), _k2(k2), _k3(k3), _k4(k4), _k5(k5)
{}

inline CameraModelPlumbBob::CameraModelPlumbBob(
   const Eigen::Vector2i& imageResolution,
   const Eigen::Vector2d& focalLength,
   const Eigen::Vector2d& principlePoint,
   double k1, double k2, double k3, double k4, double k5
) :
   CameraModelPinhole(imageResolution, focalLength, principlePoint),
   _k1(k1), _k2(k2), _k3(k3), _k4(k4), _k5(k5)
{}

inline CameraModelPlumbBob::~CameraModelPlumbBob() {}

inline Eigen::Vector2d CameraModelPlumbBob::distortPoint(const Eigen::Vector2d& undistorted)
{
   double xu = undistorted(0);
   double yu = undistorted(1);
   double xun = _invProjectionMatrix(0,0)*xu + _invProjectionMatrix(0,1)*yu + _invProjectionMatrix(0,2);
   double yun = _invProjectionMatrix(1,0)*xu + _invProjectionMatrix(1,1)*yu + _invProjectionMatrix(1,2);
   double x2 = xun * xun;
   double y2 = yun * yun;
   double xy = xun * yun;
   double r2 = x2 + y2;
   double r4 = r2 * r2;
   double r6 = r4 * r2;
   double radialDistortion(1.0 + _k1*r2 + _k2*r4 + _k5*r6);
   double tangentialDistortionX = (2 * _k3 * xy) + (_k4 * (r2 + 2 * x2));
   double tangentialDistortionY = (_k3 * (r2 + 2 * y2)) + (2 * _k4 * xy);
   double xdn = (xun * radialDistortion) + tangentialDistortionX;
   double ydn = (yun * radialDistortion) + tangentialDistortionY;
   double xd = _projectionMatrix(0,0)*xdn + _projectionMatrix(0,1)*ydn + _projectionMatrix(0,2);
   double yd = _projectionMatrix(1,0)*xdn + _projectionMatrix(1,1)*ydn + _projectionMatrix(1,2);
   Eigen::Vector2d distorted(xd, yd);
   return distorted;
}

} //namespace camera
} //namespace btl

#endif //BTL_CAMERA_CAMERAMODELPLUMBBOB
